"""
实体模块
"""

import numpy as np
from typing import Tuple, List, Optional


class Entity:
    def __init__(self, position: np.ndarray, velocity: np.ndarray, 
                 max_speed: float = 1.0, size: float = 0.5):
        self.position = np.array(position, dtype=float)
        self.velocity = np.array(velocity, dtype=float)
        self.max_speed = max_speed
        self.size = size
        self.color = (0, 0, 0)  # 默认黑色
        
    def update(self, dt: float):
        self.position += self.velocity * dt
        
    def distance_to(self, other) -> float:
        return np.linalg.norm(self.position - other.position)
    
    def is_colliding_with(self, other) -> bool:
        return self.distance_to(other) < (self.size + other.size)


class USV(Entity):
    def __init__(self, position: np.ndarray, velocity: np.ndarray, 
                 perception_radius: float = 5.0, max_speed: float = 2.0, 
                 max_force: float = 0.3, size: float = 0.5):
        super().__init__(position, velocity, max_speed, size)
        self.percept_radius = perception_radius
        self.max_force = max_force
        self.color = (0, 0, 255)  # 蓝色
        
        # 转向相关参数
        self.max_turn_rate = 0.8
        self.turn_damping=0.7
        self.prev_heading = None
        
        # Boids模型权重参数
        self.separation_weight=3.0
        self.alignment_weight = 1.0
        self.cohesion_weight = 0.7
        self.target_weight=2.2
        
        # 围捕行为参数
        self.orbit_distance = 4.0
        self.orbit_factor=1.0
        self.approach_factor = 0.7
        
        # 分离控制参数
        self.critical_separation = 2.5
        self.avoidance_factor=1.8
        
        # 角色相关参数
        self.role = None
        self.target_sector=None
        self.sector_switching_cooldown = 0.0
        
        # 速度调整参数
        self.min_speed=0.3
        self.speed_adjustment_factor = 0.8
        self.target_switch_threshold = 0.7
        self.steering_smoothing=0.3
        
        # 空隙检测相关参数
        self.gap_detection_range = 1.5 * self.percept_radius
        self.gap_angle_threshold = 0.7
        self.is_filling_gap = False
        self.gap_fill_cooldown=0.0
        
    def apply_force(self, force: np.ndarray):
        # 限制力的大小
        if np.linalg.norm(force) > self.max_force:
            force = self.max_force * force / np.linalg.norm(force)
        
        # 获取当前航向
        current_heading = None
        current_speed = np.linalg.norm(self.velocity)
        if current_speed > 0.001:
            current_heading = self.velocity / current_speed
        
        # 计算期望航向
        desired_velocity = self.velocity + force
        desired_speed = np.linalg.norm(desired_velocity)
        if desired_speed > 0.001:
            desired_heading = desired_velocity / desired_speed
        else:
            desired_heading = current_heading if current_heading is not None else np.array([1.0, 0.0])
        
        # 如果这是第一帧，初始化前一帧航向
        if self.prev_heading is None and current_heading is not None:
            self.prev_heading = current_heading
        
        # 应用转向限制
        if current_heading is not None and self.prev_heading is not None:
            # 计算期望的转向角度变化
            dot_product = np.clip(np.dot(current_heading, desired_heading), -1.0, 1.0)
            angle_change = np.arccos(dot_product)
            
            # 确定转向方向
            cross_product = np.cross(np.append(current_heading, 0), np.append(desired_heading, 0))[2]
            turn_direction = 1 if cross_product >= 0 else -1
            
            # 限制转向角速度
            max_angle_change = self.max_turn_rate
            if angle_change > max_angle_change:
                angle_change = max_angle_change
            
            # 应用转向阻尼
            angle_change = angle_change * (1.0 - self.turn_damping)
            
            # 计算新的航向
            cos_angle = np.cos(angle_change * turn_direction)
            sin_angle = np.sin(angle_change * turn_direction)
            new_heading = np.array([
                current_heading[0] * cos_angle - current_heading[1] * sin_angle,
                current_heading[0] * sin_angle + current_heading[1] * cos_angle
            ])
            
            # 应用新航向和原始速度
            desired_speed = min(desired_speed, self.max_speed)
            self.velocity = new_heading * desired_speed
            
            # 保存当前航向作为下一帧的前一帧航向
            self.prev_heading = new_heading
        else:
            # 如果没有前一帧航向，直接应用力
            self.velocity += force
            
            # 限制速度
            speed = np.linalg.norm(self.velocity)
            if speed > self.max_speed:
                self.velocity = self.velocity * self.max_speed / speed
        
    def separation(self, neighbors: List['USV']) -> np.ndarray:
        """
        计算分离力 - 避免与其他艇只过于接近
        
        :param neighbors: 周围的USV列表
        :return: 分离力向量
        """
        steering = np.zeros(2)
        count = 0
        
        for other in neighbors:
            d = self.distance_to(other)
            # 防止除零错误，同时确保只考虑在感知范围内的邻居
            if 0 < d < self.percept_radius:
                # 计算反向力，距离越近力越大
                diff = self.position - other.position
                
                # 强化小距离的分离惩罚
                separation_strength = 1.0
                if d < self.critical_separation:
                    # 当距离小于临界值时，采用更强的反向力
                    separation_strength = self.avoidance_factor * (self.critical_separation / d)
                
                # 修改距离衰减公式，使分离力在较远距离也有足够强度
                diff = diff / (d * d * 0.6) * separation_strength
                steering += diff
                count += 1
                
        if count > 0:
            steering /= count
            if np.linalg.norm(steering) > 0:
                # 设置所需的最大速度
                steering = (steering / np.linalg.norm(steering)) * self.max_speed
                # 转向力 = 期望速度 - 当前速度
                steering -= self.velocity
                # 限制转向力
                if np.linalg.norm(steering) > self.max_force:
                    steering = (steering / np.linalg.norm(steering)) * self.max_force
                    
        return steering
    
    def alignment(self, neighbors: List['USV']) -> np.ndarray:
        """
        计算对齐力 - 使艇只朝向与邻居相同的方向
        
        :param neighbors: 周围的USV列表
        :return: 对齐力向量
        """
        steering = np.zeros(2)
        count = 0
        
        for other in neighbors:
            d = self.distance_to(other)
            if 0 < d < self.percept_radius:
                steering += other.velocity
                count += 1
                
        if count > 0:
            steering /= count
            # 设置期望速度的大小
            if np.linalg.norm(steering) > 0:
                steering = (steering / np.linalg.norm(steering)) * self.max_speed
            # 转向力
            steering -= self.velocity
            # 限制转向力
            if np.linalg.norm(steering) > self.max_force:
                steering = (steering / np.linalg.norm(steering)) * self.max_force
                
        return steering
    
    def cohesion(self, neighbors: List['USV']) -> np.ndarray:
        steering = np.zeros(2)
        count = 0
        
        for other in neighbors:
            d = self.distance_to(other)
            if 0 < d < self.percept_radius:
                steering += other.position
                count += 1
                
        if count > 0:
            steering /= count
            # 向平均位置的方向
            steering -= self.position
            # 设置期望速度
            if np.linalg.norm(steering) > 0:
                steering = (steering / np.linalg.norm(steering)) * self.max_speed
            # 转向力
            steering -= self.velocity
            # 限制转向力
            if np.linalg.norm(steering) > self.max_force:
                steering = (steering / np.linalg.norm(steering)) * self.max_force
                
        return steering
    
    def seek_target(self, target, leadership_info=None) -> np.ndarray:
        if target is None:
            return np.zeros(2)
            
        # 计算到目标的向量和距离
        to_target = target.position - self.position
        dist = np.linalg.norm(to_target)
        
        if dist <= 0:  # 避免除零错误
            return np.zeros(2)
        
        # 获取朝向目标的单位向量
        to_target_norm = to_target / dist
        
        # 使用角色信息时的特殊处理
        if leadership_info is not None:
            role = leadership_info.get('role')
            assigned_sector = leadership_info.get('sector_angle')
            
            if role == 'encirclement' and assigned_sector is not None:
                return self._seek_encirclement_position(target, assigned_sector, dist)
            elif role == 'backup':
                # 备用角色处理
                return self._seek_backup_position(target, assigned_sector, dist)
        
        # 计算理想的围绕距离
        ideal_dist = target.size * self.orbit_distance
        
        # 改进的围捕逻辑
        if dist < ideal_dist * 1.8:  # 增大转换距离，使无人艇更早开始围捕行为
            # 当靠近目标时，执行围捕行为
            
            # 计算径向调整力
            radius_diff = dist - ideal_dist
            radius_force = to_target_norm * radius_diff * self.approach_factor
            
            # 计算围绕力
            perp_vector = np.array([-to_target_norm[1], to_target_norm[0]])
            
            # 切向力强度
            orbit_strength= self.orbit_factor * (1.0 - abs(radius_diff) / ideal_dist)
            orbit_strength = max(0.2, min(1.0, orbit_strength))
            
            # 根据艇只当前速度方向调整切向转向
            vel_norm = self.velocity / np.linalg.norm(self.velocity) if np.linalg.norm(self.velocity) > 0 else np.zeros(2)
            dot_product = np.dot(vel_norm, perp_vector)
            
            # 根据当前运动方向调整
            if dot_product > 0:
                orbit_force = perp_vector * self.max_speed*orbit_strength
            else:
                orbit_force = (perp_vector + vel_norm) * self.max_speed * orbit_strength * 0.5
                
            # 计算总合力
            desired_velocity=orbit_force + radius_force
            
            # 近距离减速
            if dist < ideal_dist * 0.5:
                desired_velocity *= 0.5
        else:
            # 远距离直接追击
            desired_velocity = to_target_norm * self.max_speed
            
        # 转向力
        steering = desired_velocity - self.velocity
        
        # 限制转向力
        if np.linalg.norm(steering) > self.max_force:
            steering = (steering / np.linalg.norm(steering)) * self.max_force
            
        return steering
        
    def _seek_encirclement_position(self, target, sector_angle, current_distance):
        # 计算理想距离
        ideal_dist = target.size * self.orbit_distance
        
        # 计算理想位置
        ideal_x = target.position[0] + np.cos(sector_angle) * ideal_dist
        ideal_y = target.position[1] + np.sin(sector_angle) * ideal_dist
        ideal_position = np.array([ideal_x, ideal_y])
        
        # 计算到理想位置的向量
        to_ideal = ideal_position - self.position
        dist_to_ideal = np.linalg.norm(to_ideal)
        
        if dist_to_ideal <= 0:
            return np.zeros(2)
            
        # 计算速度
        speed_factor = min(1.0, dist_to_ideal / (ideal_dist * 0.5))
        desired_velocity = (to_ideal / dist_to_ideal) * self.max_speed * speed_factor
        
        # 当非常接近理想位置时，开始绕目标旋转
        if dist_to_ideal < ideal_dist * 0.2:
            # 计算切向向量
            to_target = target.position - self.position
            to_target_norm = to_target / np.linalg.norm(to_target)
            tangent = np.array([-to_target_norm[1], to_target_norm[0]])
            
            # 混合切向和径向
            desired_velocity = desired_velocity * 0.3 + tangent * self.max_speed * 0.7
            
        # 转向力
        steering = desired_velocity - self.velocity
        
        # 限制转向力
        if np.linalg.norm(steering) > self.max_force:
            steering = (steering / np.linalg.norm(steering)) * self.max_force
            
        return steering
    
    def _seek_backup_position(self, target, sector_angle, current_distance):
        # 备用位置设置
        ideal_dist = target.size * self.orbit_distance * 1.5
        
        # 计算理想位置
        ideal_x = target.position[0] + np.cos(sector_angle) * ideal_dist
        ideal_y = target.position[1] + np.sin(sector_angle) * ideal_dist
        ideal_position = np.array([ideal_x, ideal_y])
        
        # 计算到理想位置的向量
        to_ideal = ideal_position - self.position
        dist_to_ideal = np.linalg.norm(to_ideal)
        
        if dist_to_ideal <= 0:
            return np.zeros(2)
            
        # 远距离加速
        speed_factor = 1.0
        if current_distance > ideal_dist * 2:
            speed_factor = 1.5
            
        desired_velocity = (to_ideal / dist_to_ideal) * self.max_speed * speed_factor
        
        # 转向力
        steering = desired_velocity - self.velocity
        
        # 限制转向力
        if np.linalg.norm(steering) > self.max_force:
            steering = (steering / np.linalg.norm(steering)) * self.max_force
            
        return steering
        
    def detect_encirclement_gaps(self, target, neighbors: List['USV']) -> Tuple[bool, Optional[np.ndarray]]:
        """
        检测目标周围的包围圈是否存在空隙
        
        :param target: 目标实体
        :param neighbors: 周围的USV列表
        :return: (是否检测到空隙, 空隙位置向量)
        """
        if target is None or len(neighbors) < 3:
            return False, None
            
        # 计算到目标的向量和距离
        to_target = target.position - self.position
        distance = np.linalg.norm(to_target)
        
        # 如果距离太远，不考虑填补空隙
        if distance > self.gap_detection_range:
            return False, None
            
        # 获取所有无人艇相对于目标的角度
        usv_angles = []
        for usv in neighbors:
            if usv is self:  # 跳过自己
                continue
                
            # 计算该USV到目标的向量
            usv_to_target = target.position - usv.position
            usv_distance = np.linalg.norm(usv_to_target)
            
            # 只考虑围绕目标一定范围内的USV
            min_range = target.size + 1.5
            max_range = target.size + self.orbit_distance * 1.5
            
            if min_range <= usv_distance <= max_range:
                # 计算角度(0-2π)
                angle = np.arctan2(usv_to_target[1], usv_to_target[0]) % (2 * np.pi)
                usv_angles.append(angle)
                
        # 如果周围没有足够的USV，无法判断空隙
        if len(usv_angles) < 2:
            return False, None
            
        # 对角度排序
        usv_angles.sort()
        
        # 添加首尾相连的角度，便于检测
        usv_angles.append(usv_angles[0] + 2 * np.pi)
        
        # 查找最大的角度差，即空隙
        max_gap = 0
        gap_start_angle = 0
        
        for i in range(len(usv_angles) - 1):
            gap = usv_angles[i+1] - usv_angles[i]
            if gap > max_gap:
                max_gap = gap
                gap_start_angle = usv_angles[i]
                
        # 如果最大角度差超过阈值，认为存在空隙
        if max_gap > self.gap_angle_threshold:
            # 计算空隙中心角度
            gap_center_angle = (gap_start_angle + max_gap / 2) % (2 * np.pi)
            
            # 计算空隙位置 - 在理想围绕距离上
            ideal_distance = target.size + self.orbit_distance
            gap_position = np.array([
                target.position[0] + ideal_distance * np.cos(gap_center_angle),
                target.position[1] + ideal_distance * np.sin(gap_center_angle)
            ])
            
            # 检查我是否是最接近空隙的USV
            my_angle = np.arctan2(to_target[1], to_target[0]) % (2 * np.pi)
            my_angle_to_gap = min((gap_center_angle - my_angle) % (2 * np.pi), 
                                  (my_angle - gap_center_angle) % (2 * np.pi))
            
            # 计算其他USV到空隙的角度距离
            i_am_closest = True
            for usv in neighbors:
                if usv is self:
                    continue
                    
                usv_to_target = target.position - usv.position
                usv_angle = np.arctan2(usv_to_target[1], usv_to_target[0]) % (2 * np.pi)
                usv_angle_to_gap = min((gap_center_angle - usv_angle) % (2 * np.pi), 
                                      (usv_angle - gap_center_angle) % (2 * np.pi))
                
                # 如果另一个USV角度上更接近空隙，且距离目标不太远
                if usv_angle_to_gap < my_angle_to_gap and usv.distance_to(target) < self.gap_detection_range:
                    i_am_closest = False
                    break
            
            # 如果我是最接近空隙的，或者随机选择我来填补空隙
            if i_am_closest or np.random.random() < 0.2:  # 20%概率随机选择，增加多样性
                return True, gap_position
                
        return False, None
        
    def adjust_speed_for_gap_filling(self, is_filling_gap: bool, distance_to_target: float) -> float:
        """
        根据是否填补空隙调整速度
        
        :param is_filling_gap: 是否正在填补空隙
        :param distance_to_target: 到目标的距离
        :return: 调整后的速度系数(0-1)
        """
        if not is_filling_gap:
            # 正常情况下，速度随距离增加
            if distance_to_target < self.orbit_distance * 0.7:
                # 当接近目标时，减速
                return 0.7
            else:
                # 正常速度
                return 1.0
        else:
            # 填补空隙时，根据距离调整速度
            orbit_range = self.orbit_distance * 1.2
            
            if distance_to_target < orbit_range:
                # 接近理想位置时，大幅减速
                return 0.5
            elif distance_to_target < orbit_range * 1.5:
                # 在中等距离时，中等减速
                return 0.7
            else:
                # 距离较远时，小幅减速
                return 0.9
        
    def update(self, dt: float, usvs: List['USV'], target=None, leadership_info=None):
        """
        更新无人艇状态
        
        :param dt: 时间步长
        :param usvs: 所有USV列表
        :param target: 分配的目标
        :param leadership_info: 领导信息，包含角色和扇区分配
        """
        # 更新角色切换冷却时间
        if self.sector_switching_cooldown > 0:
            self.sector_switching_cooldown -= dt
            
        # 更新空隙填补冷却时间
        if self.gap_fill_cooldown > 0:
            self.gap_fill_cooldown -= dt
            self.is_filling_gap = self.gap_fill_cooldown > 0
            
        # 获取感知范围内的邻居
        neighbors = []
        for other in usvs:
            if other is not self and self.distance_to(other) < self.percept_radius:
                neighbors.append(other)

        # 应用标准Boids行为规则
        sep_force = self.separation(neighbors) * self.separation_weight
        align_force = self.alignment(neighbors) * self.alignment_weight
        coh_force = self.cohesion(neighbors) * self.cohesion_weight
        
        # 计算目标力
        target_force = np.zeros(2)
        if target is not None:
            target_force = self.seek_target(target, leadership_info) * self.target_weight
            
        # 检测包围圈空隙
        detected_gap = False
        gap_position = None
        
        # 只在冷却时间结束后才检测空隙
        if self.gap_fill_cooldown <= 0 and target is not None:
            detected_gap, gap_position = self.detect_encirclement_gaps(target, usvs)
            
            if detected_gap:
                # 设置空隙填补状态和冷却时间
                self.is_filling_gap = True
                self.gap_fill_cooldown = 5.0  # 填补空隙5秒
                
        # 如果正在填补空隙，创建一个力指向空隙位置
        gap_force = np.zeros(2)
        if self.is_filling_gap and gap_position is not None:
            to_gap = gap_position - self.position
            gap_distance = np.linalg.norm(to_gap)
            
            if gap_distance > 0:
                # 创建指向空隙的力
                gap_force = (to_gap / gap_distance) * self.max_speed
                gap_force -= self.velocity
                
                # 限制力的大小
                if np.linalg.norm(gap_force) > self.max_force:
                    gap_force = (gap_force / np.linalg.norm(gap_force)) * self.max_force
                    
                # 让空隙力替代目标力
                target_force = gap_force * 1.5  # 增强空隙力，确保优先填补空隙
                
        # 计算合力
        total_force = sep_force + align_force + coh_force + target_force
        
        # 应用转向平滑
        if np.linalg.norm(total_force) > 0 and self.steering_smoothing > 0:
            current_direction = self.velocity / np.linalg.norm(self.velocity) if np.linalg.norm(self.velocity) > 0 else np.zeros(2)
            desired_direction = total_force / np.linalg.norm(total_force)
            
            # 平滑转向
            smoothed_direction = current_direction * self.steering_smoothing + desired_direction * (1 - self.steering_smoothing)
            if np.linalg.norm(smoothed_direction) > 0:
                smoothed_direction = smoothed_direction / np.linalg.norm(smoothed_direction)
                total_force = smoothed_direction * np.linalg.norm(total_force)
        
        # 应用合力 - 考虑物理特性的转向限制
        self.apply_force(total_force * dt)  # 这里添加dt因子，保持力的正确比例
        
        # 调整速度
        speed_factor = 1.0
        if target is not None:
            distance_to_target = self.distance_to(target)
            speed_factor = self.adjust_speed_for_gap_filling(self.is_filling_gap, distance_to_target)
            
        # 应用速度调整 - 保持速度在最小值和最大值之间
        current_speed = np.linalg.norm(self.velocity)
        if current_speed > 0:
            adjusted_speed = max(self.min_speed, min(self.max_speed, current_speed * speed_factor))
            self.velocity = (self.velocity / current_speed) * adjusted_speed
        
        # 更新位置
        super().update(dt)


class Target(Entity):
    """
    目标类，如漂浮垃圾、敌方船只等
    """
    
    def __init__(self, position: np.ndarray, velocity: np.ndarray, 
                 max_speed: float = 0.5, size: float = 0.8,
                 is_fleeing: bool = False):
        """
        初始化目标对象
        
        :param position: 初始位置
        :param velocity: 初始速度
        :param max_speed: 最大速度
        :param size: 目标尺寸
        :param is_fleeing: 是否具有逃逸行为
        """
        super().__init__(position, velocity, max_speed, size)
        self.color = (255, 0, 0)  # 红色
        self.is_fleeing = is_fleeing
        self.flee_radius = 12.0  # 增大逃逸触发半径（原为10.0）
        self.flee_strength = 1.2  # 增强逃逸行为强度（原为1.0）
        
        # 目标ID，用于围捕者的分配
        self.id = id(self)  # 使用对象ID作为唯一标识
        
        # 速度控制参数
        self.normal_max_speed = max_speed  # 正常最大速度
        self.min_speed = max_speed * 0.25  # 减小最小速度，使更容易被逼停（原为0.35）
        self.caution_radius = 20.0  # 增大开始减速的距离（原为18.0）
        self.slowdown_radius = 10.0  # 稍微增大最大减速的距离（原为9.0）
        self.acceleration = 0.04    # 减小加速度，使其更难恢复速度（原为0.05）
        self.deceleration = 0.12    # 增大减速度，使减速更快速（原为0.08）
        
        # 围捕判定参数
        self.capture_speed_threshold = max_speed * 0.3  # 降低速度阈值，更容易被判定为捕获（原为0.4）
        self.is_captured = False  # 是否已被围捕标志
        
        # 围捕持续时间计数
        self.capture_counter = 0.0  # 满足围捕条件的持续时间
        self.capture_time_threshold = 1.5  # 减少需要持续满足围捕条件的时间（原为2.0秒）
        
        # 逃避能力随时间衰减系数
        self.flee_energy = 1.0
        self.flee_energy_depletion_rate = 0.03  # 逃避能量消耗率
        self.flee_energy_recovery_rate = 0.01  # 逃避能量恢复率
        
        # 禁用捕获标志 - 如果为True，则目标永远不会被标记为已捕获
        self.disable_capture = False
        
    def flee(self, pursuers: List[USV]) -> np.ndarray:
        """
        计算逃逸转向力
        
        :param pursuers: 周围的USV列表
        :return: 逃逸力向量
        """
        if not self.is_fleeing or self.flee_energy <= 0.1:  # 能量不足时减少逃避行为
            return np.zeros(2)
            
        flee_force = np.zeros(2)
        
        # 查找最近的追捕者及其数量
        close_pursuers = []
        very_close_pursuers = []
        
        for usv in pursuers:
            d = self.distance_to(usv)
            if d < self.flee_radius:
                close_pursuers.append((usv, d))
                if d < self.flee_radius * 0.6:  # 增大非常近的判定范围（原为0.5）
                    very_close_pursuers.append((usv, d))
        
        # 如果没有附近的追捕者，保持当前速度
        if not close_pursuers:
            return np.zeros(2)
        
        # 重点躲避非常近的追捕者
        if very_close_pursuers:
            # 按距离排序，优先躲避最近的
            very_close_pursuers.sort(key=lambda x: x[1])
            
            # 对最近的几个追捕者计算逃逸力
            max_count = min(3, len(very_close_pursuers))
            for i in range(max_count):
                usv, d = very_close_pursuers[i]
                # 远离追捕者的方向
                diff = self.position - usv.position
                if np.linalg.norm(diff) > 0:
                    # 距离越近，逃逸力越大
                    strength = (self.flee_radius - d) / self.flee_radius
                    flee_force += diff / np.linalg.norm(diff) * strength * 2.5  # 增大近距离逃逸力（原为2.0）
        else:
            # 计算所有追捕者的平均位置
            avg_position = np.zeros(2)
            for usv, _ in close_pursuers:
                avg_position += usv.position
            avg_position /= len(close_pursuers)
            
            # 远离平均位置
            diff = self.position - avg_position
            if np.linalg.norm(diff) > 0:
                flee_force = diff / np.linalg.norm(diff) * self.max_speed
                
            # 检查逃逸方向是否会导致更多追捕者
            # 计算前方区域是否有更多追捕者
            forward_count = 0
            forward_zone_angle = np.pi / 3  # 60度前方扇区
            
            if np.linalg.norm(self.velocity) > 0:
                heading = self.velocity / np.linalg.norm(self.velocity)
                
                for usv in pursuers:
                    if self.distance_to(usv) < self.flee_radius * 1.5:  # 检查更大范围
                        to_usv = usv.position - self.position
                        if np.linalg.norm(to_usv) > 0:
                            to_usv_norm = to_usv / np.linalg.norm(to_usv)
                            angle = np.arccos(np.clip(np.dot(heading, to_usv_norm), -1.0, 1.0))
                            if angle < forward_zone_angle:
                                forward_count += 1
                
                # 如果前方有多个追捕者，调整逃逸方向
                if forward_count >= 2:
                    # 计算垂直于当前航向的方向
                    perp_dir = np.array([-heading[1], heading[0]])
                    # 随机选择左右方向
                    if np.random.random() > 0.5:
                        perp_dir = -perp_dir
                    # 混合原始逃逸方向和垂直方向
                    if np.linalg.norm(flee_force) > 0:
                        flee_force = 0.3 * flee_force + 0.7 * perp_dir * self.max_speed
        
        # 加入更多随机性，避免可预测的逃逸路径
        if np.random.random() < 0.15:  # 增加随机性概率（原为0.1）
            random_angle = np.random.uniform(-np.pi/3, np.pi/3)  # 增大随机角度范围（原为±45度）
            c, s = np.cos(random_angle), np.sin(random_angle)
            # 旋转当前逃逸力
            if np.linalg.norm(flee_force) > 0:
                x, y = flee_force
                flee_force = np.array([c*x - s*y, s*x + c*y])
                
        # 根据剩余能量调整逃避强度
        energy_factor = min(1.0, max(0.2, self.flee_energy))
        return flee_force * self.flee_strength * energy_factor
    
    def adjust_speed_based_on_surroundings(self, pursuers: List[USV]) -> float:
        """
        根据周围无人艇的分布情况调整目标速度
        
        :param pursuers: 追捕者列表
        :return: 应用的最大速度
        """
        if not pursuers:
            return self.normal_max_speed
        
        # 1. 找出最近的追捕者距离
        min_distance = float('inf')
        for usv in pursuers:
            d = self.distance_to(usv)
            min_distance = min(min_distance, d)
        
        # 2. 检查是否被包围(计算周围无人艇的角度分布)
        if len(pursuers) >= 3:  # 至少需要3艘无人艇才可能形成包围
            angles = []
            nearby_usvs = []
            
            # 只考虑真正接近目标的无人艇进行包围判定
            for usv in pursuers:
                d = self.distance_to(usv)
                if d < self.caution_radius:
                    # 计算角度
                    dx = usv.position[0] - self.position[0]
                    dy = usv.position[1] - self.position[1]
                    angle = np.arctan2(dy, dx)
                    angles.append(angle)
                    nearby_usvs.append(usv)
            
            # 对角度进行排序
            angles.sort()
            
            # 检查角度间隔是否表明被包围
            surrounded = False
            if len(angles) >= 3:
                # 添加首个角度到末尾，以形成闭环
                angles.append(angles[0] + 2 * np.pi)
                
                # 检查最大间隔
                max_gap = 0
                for i in range(len(angles) - 1):
                    gap = angles[i+1] - angles[i]
                    max_gap = max(max_gap, gap)
                
                # 进一步放宽包围条件，允许更大的缺口
                if max_gap <= 2.8:  # 约160度（原为2.6，约150度）
                    surrounded = True
                
                # 如果有足够多的无人艇(4+)，即使有更大的间隙也可视为被包围（原为5+）
                if len(nearby_usvs) >= 4 and max_gap <= 3.8:  # 约220度（原为3.5，约200度）
                    surrounded = True
        else:
            surrounded = False
            
        # 3. 根据距离和包围状态调整速度
        if min_distance < self.slowdown_radius * 0.6 or surrounded:  # 原为0.7
            # 在极近距离或被包围时，降至最低速度
            # 如果禁用捕获，使用较高的最低速度确保持续移动
            if hasattr(self, 'disable_capture') and self.disable_capture:
                target_speed = max(self.min_speed * 1.5, self.normal_max_speed * 0.4)
            else:
                target_speed = self.min_speed
        elif min_distance < self.caution_radius:
            # 在警戒范围内，使用更陡峭的减速曲线
            slowdown_factor = (min_distance - self.slowdown_radius) / (self.caution_radius - self.slowdown_radius)
            slowdown_factor = max(0.0, slowdown_factor) ** 1.2  # 使减速曲线更陡峭（原为0.8）
            target_speed = self.min_speed + (self.normal_max_speed - self.min_speed) * slowdown_factor
        else:
            # 安全距离外，恢复正常速度
            target_speed = self.normal_max_speed
            
        return target_speed
    
    def update(self, dt: float, pursuers: List[USV] = None):
        """
        更新目标状态，可能包括逃逸行为和减速行为
        
        :param dt: 时间步长
        :param pursuers: 潜在的追捕者列表
        """
        if pursuers is not None and len(pursuers) > 0:
            # 调整最大速度
            target_max_speed = self.adjust_speed_based_on_surroundings(pursuers)
            
            # 平滑过渡到目标速度
            if target_max_speed < self.max_speed:
                # 减速
                self.max_speed = max(target_max_speed, self.max_speed - self.deceleration * dt)
            else:
                # 加速
                self.max_speed = min(target_max_speed, self.max_speed + self.acceleration * dt)
            
            # 更新逃避能量
            pursuing_usv_count = sum(1 for usv in pursuers if self.distance_to(usv) < self.flee_radius)
            
            # 在被追捕时消耗能量
            if pursuing_usv_count > 0 and self.is_fleeing:
                # 根据追捕者数量增加消耗
                depletion = self.flee_energy_depletion_rate * (1 + 0.2 * (pursuing_usv_count - 1))
                self.flee_energy = max(0.0, self.flee_energy - depletion * dt)
            else:
                # 没有追捕者时恢复能量
                self.flee_energy = min(1.0, self.flee_energy + self.flee_energy_recovery_rate * dt)
            
            # 计算当前速度是否已经足够低，可以判定为被逼停/捕获
            current_speed = np.linalg.norm(self.velocity)
            nearby_usv_count = sum(1 for usv in pursuers if self.distance_to(usv) < self.caution_radius * 0.8)
            
            # 捕获条件：
            # 1. 速度足够低 且 周围有足够多的追捕者(至少3艘) 
            # 2. 或者 特别低的速度(几乎静止)且有至少1艘追捕者在附近
            # 3. 或者 能量几乎耗尽(<15%)且有至少2艘追捕者在附近
            capture_condition = (current_speed <= self.capture_speed_threshold and nearby_usv_count >= 3) or \
                                (current_speed <= self.capture_speed_threshold * 0.5 and nearby_usv_count >= 1) or \
                                (self.flee_energy < 0.15 and nearby_usv_count >= 2)
            
            # 如果禁用捕获机制，则忽略捕获条件
            if hasattr(self, 'disable_capture') and self.disable_capture:
                capture_condition = False
            
            if capture_condition:
                # 增加捕获计数器，检查是否持续满足捕获条件
                self.capture_counter += dt
                if self.capture_counter >= self.capture_time_threshold:
                    if not self.is_captured:
                        self.is_captured = True
                        self.color = (128, 0, 0)  # 深红色表示已被捕获
                        # 捕获时强制减速到非常低的速度
                        current_speed = np.linalg.norm(self.velocity)
                        if current_speed > 0:
                            self.velocity = self.velocity / current_speed * (self.min_speed * 0.1)
            else:
                # 不满足条件时逐渐重置计数器，但重置速度慢一些
                self.capture_counter = max(0.0, self.capture_counter - dt * 1.0)  # 原来是1.5
                
            # 计算逃逸力
            if self.is_fleeing and not self.is_captured:  # 被捕获后停止逃逸
                flee_force = self.flee(pursuers)
                
                # 限制速度
                current_speed = np.linalg.norm(self.velocity)
                if current_speed > 0:
                    self.velocity = self.velocity / current_speed * self.max_speed
                    
                # 应用逃逸力
                self.velocity += flee_force
                
                # 再次限制速度
                current_speed = np.linalg.norm(self.velocity)
                if current_speed > self.max_speed:
                    self.velocity = self.velocity / current_speed * self.max_speed
        
        # 根据是否被捕获决定更新位置
        if self.is_captured:
            # 被捕获后几乎停止移动，但保留微小移动以模拟漂移
            self.velocity *= 0.01  # 进一步减小，原为0.02
            # 确保速度不为零，保持微小漂移
            if np.linalg.norm(self.velocity) < 0.001:
                angle = np.random.uniform(0, 2 * np.pi)
                self.velocity = np.array([np.cos(angle), np.sin(angle)]) * 0.001
            super().update(dt * 0.05)  # 减小时间步长，进一步减少移动（原为0.1）
        else:
            # 未被捕获时正常更新
            super().update(dt) 
